#include <iostream>
#include <cstdint>
#include <cstring>

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include "SerialPort/SerialPort.h"

using namespace std;

ros::Publisher mcu_serial_pub;
ros::Subscriber mcu_serial_sub;
ros::Subscriber vision_sub;

SerialPort serial("/dev/ttyUSB0", B115200);

void mcu_serial_Callback(const std_msgs::String::ConstPtr& msg)
{
    std::string r_msg = msg->data;
    serial.sendData(r_msg);
    ROS_INFO("I receive from PC:%s", r_msg.c_str());
}

void vision_Callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    stringstream ss;
    string color;
    if (msg->pose.position.x == 1)
        color = "r";
    else if (msg->pose.position.x == 2)
        color = "g";
    else if (msg->pose.position.x == 3)
        color = "b";
    else if (msg->pose.position.x == 4)
        color = "y";
    else
        color = "w";
    ss << "COLOR_SHAPE_ERROR:" << color << "," << msg->pose.position.y << "," << msg->pose.position.z << "," << msg->pose.orientation.w << "\r\n";
    string msg2mcu = ss.str();
    if (color != "w"){
        serial.sendData(msg2mcu);
        ROS_INFO("I receive from PC:%s", msg2mcu.c_str());
    }
        
}



int main(int argc, char **argv) {
    if (!serial.openPort()) {
        return 1;
    }
    cout << "Port /dev/ttyACM0 opened successfully!\n" << endl;
    // ROS节点初始化
    ros::init(argc, argv, "mcu_serial_publisher");

    // 创建节点句柄 
    ros::NodeHandle nh;
    mcu_serial_pub = nh.advertise<std_msgs::String>("/MCU_TO_PC", 10);
    mcu_serial_sub = nh.subscribe("/PC_TO_MCU", 10, mcu_serial_Callback);
    vision_sub = nh.subscribe("/ele_vision", 10, vision_Callback);

    ros::Rate rate(100);
    while(ros::ok())
    {
        std::string msg = serial.receiveData();
        if (!msg.empty())
        {
            std_msgs::String String_pub;
            String_pub.data = msg;
            mcu_serial_pub.publish(String_pub);

            ROS_INFO("I receive from MCU:%s", msg.c_str());
        }
        ros::spinOnce();
        rate.sleep();
    }

    serial.closePort();

    return 0;
}